// Quat.cpp: implementation of the Quat class.
//
//////////////////////////////////////////////////////////////////////

#include "Quat.h"

#include <stdio.h>
#include "Quat.h"
#include "Matrix.h"
#include "Vec4.h"
#include "Vec3.h"
//#include <math.h>

// Good introductions to Quaternions at:
// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
// http://mathworld.wolfram.com/Quaternion.html
// Set the elements of the Quat to represent a rotation of angle
// (radians) around the axis (x,y,z)

void Quat::makeRotate( float angle, float x, float y, float z )
{
    float inversenorm  = 1.0/sqrt( x*x + y*y + z*z );
    float coshalfangle = cos( 0.5*angle );
    float sinhalfangle = sin( 0.5*angle );

    vec[0] = x * sinhalfangle * inversenorm;
    vec[1] = y * sinhalfangle * inversenorm;
    vec[2] = z * sinhalfangle * inversenorm;
    vec[3] = coshalfangle;
}

void Quat::makeRotate( float angle, const Vec3& vec )
{
    makeRotate( angle, vec[0], vec[1], vec[2] );
}

void Quat::makeRotate ( float angle1, const Vec3& axis1, 
                        float angle2, const Vec3& axis2,
                        float angle3, const Vec3& axis3)
{
    Quat q1; q1.makeRotate(angle1,axis1);
    Quat q2; q2.makeRotate(angle2,axis2);
    Quat q3; q3.makeRotate(angle3,axis3);

    *this = q1*q2*q3;
}                        

// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.

void Quat::makeRotate( const Vec3& from, const Vec3& to )
{
    const float epsilon = 0.00001f;
    float length1  = from.length();
    float length2  = to.length();
    
    // dot product vec1*vec2
    float cosangle = from*to/(length1*length2);

    if ( fabs(cosangle - 1) < epsilon )
    {
        // cosangle is close to 1, so the vectors are close to being coincident
        // Need to generate an angle of zero with any vector we like
        // We'll choose (1,0,0)
        makeRotate( 0.0, 1.0, 0.0, 0.0 );
    }
    else if ( fabs(cosangle + 1.0) < epsilon )
    {
        // vectors are close to being opposite, so will need to find a
        // vector orthongonal to from to rotate about.
        Vec3 tmp;
        if (fabs(from.x())<fabs(from.y()))
            if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
            else tmp.set(0.0,0.0,1.0);
        else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
        else tmp.set(0.0,0.0,1.0);
        
        // find orthogonal axis.
        Vec3 axis(from^tmp);
        axis.normalize();
        
        vec[0] = axis[0]; // sin of half angle of PI is 1.0.
        vec[1] = axis[1]; // sin of half angle of PI is 1.0.
        vec[2] = axis[2]; // sin of half angle of PI is 1.0.
        vec[3] = 0; // cos of half angle of PI is zero.
    }
    else
    {
        // This is the usual situation - take a cross-product of vec1 and vec2
        // and that is the axis around which to rotate.
        Vec3 axis(from^to);
        float angle = acos( cosangle );
        makeRotate( angle, axis );
    }
}

void Quat::getRotate( float& angle, Vec3& vec ) const
{
    getRotate(angle,vec[0],vec[1],vec[2]);
}

// Get the angle of rotation and axis of this Quat object.
// Won't give very meaningful results if the Quat is not associated
// with a rotation!

void Quat::getRotate( float& angle, float& x, float& y, float& z ) const
{
    float sinhalfangle = sqrt( vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2] );

    angle = 2 * atan2( sinhalfangle, vec[3] );
    if(sinhalfangle)
    {
        x = vec[0] / sinhalfangle;
        y = vec[1] / sinhalfangle;
        z = vec[2] / sinhalfangle;
    }
    else
    {
        x = 0.0f;
        y = 0.0f;
        z = 1.0f;
    }

}

/// Spherical Linear Interpolation
/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
/// Reference: Shoemake at SIGGRAPH 89
/// See also
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm

void Quat::slerp( float t, const Quat& from, const Quat& to )
{
    const double epsilon = 0.00001;
    double omega, cosomega, sinomega, scale_from, scale_to ;
    
    Quat quatTo(to);
    
	// this is a dot product
    cosomega = from.asVec4() * to.asVec4();
    
    if ( cosomega < 0.0 )
    { 
        cosomega = -cosomega; 
        quatTo.set(-to.vec);
    }

    if( (1.0 - cosomega) > epsilon )
    {
        omega= acos(cosomega) ;  // 0 <= omega <= Pi (see man acos)
        sinomega = sin(omega) ;  // this sinomega should always be +ve so
        // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
        scale_from = sin((1.0-t)*omega)/sinomega ;
        scale_to = sin(t*omega)/sinomega ;
    }
    else
    {
        // The ends of the vectors are very close we can use simple linear 
		// interpolation - no need to worry about the "spherical" interpolation
        scale_from = 1.0 - t ;
        scale_to = t ;
    }

                                 // use Vec4 arithmetic
    vec = (from.vec*scale_from) + (quatTo.vec*scale_to);
    // so that we get a Vec4
}

void Quat::set( const Matrix& m )
{
    // Source: Gamasutra, Rotating Objects Using Quaternions
    // http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm

    float	tr, s;
    float	tq[4];
    int		i, j, k;
    int		nxt[3] = {1, 2, 0};

    tr = m(0,0) + m(1,1) + m(2,2);

    // check the diagonal
    if (tr > 0.0)
    {
        s = (float)sqrt (tr + 1.0);
        vec[3] = s / 2.0f;
        s = 0.5f / s;
        vec[0] = (m(1,2) - m(2,1)) * s;
        vec[1] = (m(2,0) - m(0,2)) * s;
        vec[2] = (m(0,1) - m(1,0)) * s;
    }
    else
    {
        // diagonal is negative
        i = 0;
        if (m(1,1) > m(0,0))
            i = 1;
        if (m(2,2) > m(i,i))
            i = 2;
        j = nxt[i];
        k = nxt[j];

        s = (float)sqrt ((m(i,i) - (m(j,j) + m(k,k))) + 1.0);

        tq[i] = s * 0.5f;

        if (s != 0.0f)
            s = 0.5f / s;

        tq[3] = (m(j,k) - m(k,j)) * s;
        tq[j] = (m(i,j) + m(j,i)) * s;
        tq[k] = (m(i,k) + m(k,i)) * s;

        vec[0] = tq[0];
        vec[1] = tq[1];
        vec[2] = tq[2];
        vec[3] = tq[3];
    }
}

void Quat::get( Matrix& m ) const
{
    // Source: Gamasutra, Rotating Objects Using Quaternions
    //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm

    double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

    // calculate coefficients
    x2 = vec[0] + vec[0];
    y2 = vec[1] + vec[1];
    z2 = vec[2] + vec[2];

    xx = vec[0] * x2;
    xy = vec[0] * y2;
    xz = vec[0] * z2;

    yy = vec[1] * y2;
    yz = vec[1] * z2;
    zz = vec[2] * z2;

    wx = vec[3] * x2;
    wy = vec[3] * y2;
    wz = vec[3] * z2;

    // Note.  Gamasutra gets the matrix assignments inverted, resulting
    // in left-handed rotations, which is contrary to OpenGL and OSG's 
    // methodology.  The matrix assignment has been altered in the next
    // few lines of code to do the right thing.
  
    m(0,0) = 1.0f - (yy + zz);
    m(1,0) = xy - wz;
    m(2,0) = xz + wy;
    m(3,0) = 0.0f;

    m(0,1) = xy + wz;
    m(1,1) = 1.0f - (xx + zz);
    m(2,1) = yz - wx;
    m(3,1) = 0.0f;

    m(0,2) = xz - wy;
    m(1,2) = yz + wx;
    m(2,2) = 1.0f - (xx + yy);
    m(3,2) = 0.0f;

    m(0,3) = 0;
    m(1,3) = 0;
    m(2,3) = 0;
    m(3,3) = 1;
}

// For testing only 

#ifdef USE_UNIT_TESTS
void test_Quat_Eueler(float heading,float pitch,float roll)
{
    Quat q;
    q.makeRotate(heading,pitch,roll);
    
    Matrix q_m;
    q.get(q_m);
    
    Vec3 xAxis(1,0,0);
    Vec3 yAxis(0,1,0);
    Vec3 zAxis(0,0,1);
    
    cout << "heading = "<<heading<<"  pitch = "<<pitch<<"  roll = "<<roll<<endl;

    cout <<"q_m = "<<q_m;
    cout <<"xAxis*q_m = "<<xAxis*q_m << endl;
    cout <<"yAxis*q_m = "<<yAxis*q_m << endl;
    cout <<"zAxis*q_m = "<<zAxis*q_m << endl;
    
    Matrix r_m =	Matrix::rotate(roll,0.0,1.0,0.0)*
					Matrix::rotate(pitch,1.0,0.0,0.0)*
                    Matrix::rotate(-heading,0.0,0.0,1.0);
                      
    cout << "r_m = "<<r_m;
    cout <<"xAxis*r_m = "<<xAxis*r_m << endl;
    cout <<"yAxis*r_m = "<<yAxis*r_m << endl;
    cout <<"zAxis*r_m = "<<zAxis*r_m << endl;
    
    cout << endl<<"*****************************************" << endl<< endl;
    
}

void test_Quat()
{

    test_Quat_Eueler(DegreesToRadians(20),0,0);
    test_Quat_Eueler(0,DegreesToRadians(20),0);
    test_Quat_Eueler(0,0,DegreesToRadians(20));
    test_Quat_Eueler(DegreesToRadians(20),DegreesToRadians(20),DegreesToRadians(20));
    return 0;
}

#endif
